#include "math/GaussFilter.h"
#include "math/float2int.h"
#include "math/pi.h"
#include "xtd/AlibAlgo.h"
#include "cinline.h"

#pragma warning(disable: 4244)

int GaussFilter::GetFilterLength(int nFilter, double sigma, double dSigma)
{
  int t = round(4.0*(sigma + (nFilter/2)*dSigma));
  return t > 1 ? t : 2; 
}


double GaussFilter::ValidateParameter(double sigma)
{
  return limit(sigma, 0.0, 500.0);
}


int GaussFilter::Type() const
{ 
  return AFilter::GAUSS;
}


//--------------------------------------------------------------------------
// Function computes smoothing and differentiating filters with specified
// parameter. Normalization constraints:
//
// sf[]: Integral[-inf,+inf](exp(-x^2/(2*sigma^2)) dx) == 1.
//
// df[]: Integral[0,+inf](x*exp(-x^2/(2*sigma^2)) dx) == 1/(sigma*sqrt(2*PI))
//--------------------------------------------------------------------------
void GaussFilter::ComputeFilter(float * sf, float * df, float * ddf, double sigma)
{
  double sScale = 0.0;
  double SqSigma= sqr(sigma);
  double revSqSigma = (-1.0)/(2.0*SqSigma);
  double step = 0.001;
  double halfStep = 0.5*step;

  int k;
  for(k = 0; k < m_size; k++)
  {
    double x1 = k-0.5;
    double x2 = k+0.5;
    double sSum = 0.0;
    double dSum = 0.0;
    double ddSum= 0.0;

    for(double x = x1; x < (x2-std::numeric_limits<float>::epsilon()); x += step)
    {
      double t = x + halfStep;
      double e = exp(t*t*revSqSigma);
      sSum += e;
      if (k > 0 || t >= 0.0)
        dSum += t*e;
	  ddSum += (t*t-SqSigma)*e;
    }
    sf[k] = (float)sSum;
    df[k] = (float)dSum;
    ddf[k] = (float)ddSum;
    sScale += (k == 0) ? sSum : (sSum+sSum);
  }
  df[0] = 0.0f;

  sScale = (sScale > std::numeric_limits<float>::epsilon()) ? (1.0/sScale) : 0.0;
  xtd::multiply(sf, sf+m_size, (float)sScale);

  float d = 2.0f*xtd::moment1(df, df+m_size, 0.0f);
  d = (d > std::numeric_limits<float>::epsilon()) ? (1.0/d) : 0.0;
  xtd::multiply(df, df+m_size, d);

  d = xtd::moment2(ddf, ddf+m_size, 0.0f);
  d = (d > std::numeric_limits<float>::epsilon()) ? (1.0/d) : 0.0;
  xtd::multiply(ddf, ddf+m_size, d);

//  float s0 = sf[0] + 2.0f*(xtd::moment0(sf+1, sf+m_size, 0.0f));//1.0
//  float s2 = 2*xtd::moment2(sf, sf+m_size, 0.0f)/SqSigma;		//1.0
//  float d1 = 2.0f*xtd::moment1(df, df+m_size, 0.0f);			//1.0
//  float dd0 = ddf[0]+2.0f*xtd::moment0(ddf+1, ddf+m_size, 0.0f);//0.0
//  float dd2 = xtd::moment2(ddf, ddf+m_size, 0.0f);			    //1.0
}
